#include <iostream>
#include "flight_control_sample.hpp"
#include "opencv2/opencv.hpp"
#include <opencv2/core.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;
using namespace cv;
using namespace std;
/**
 * @brief 获取sbus的x2通道信号后判断，rcmode处于p模式即大于7000，进入
 *  onboard模式夺取控制，50hz输入控制信息，如果没有发现标签就低速直飞，发现后移动到标签之上，设置在标
 *  签处停留几秒后，控制一个gpio通道点灯，从而告诉飞手到达指定位置可以关闭onboard模式
 * 
 * @param argc 
 * @param argv 
 * @return int 
 */
int main(int argc, char** argv)
{
  // Initialize variables
  int rcMode;
  int16_t sbusData[16];
  int switchChannl;//sbus第8通道对应右手拨杆，其数值为172 1118 992
  int functionTimeout = 1;//操作使用
  int responseTimeout = 1;//订阅使用
  float dx, dy, dz, dyaw;
  float Vx, Vy, Vz, yawRate;
  int pitchRate = 0;
  bool AuthorityFlag = false;
  // Setup OSDK.
  LinuxSetup linuxEnvironment(argc, argv);
  std::cout<<"environment Done"<<std::endl;
  Vehicle*   vehicle = linuxEnvironment.getVehicle();
  std::cout<<"get vehicle Done"<<std::endl;
  if (vehicle == NULL)
  {
    std::cout << "Vehicle not initialized, exiting.\n";
    return -1;
  }
  dx = 1;
  dy = 1;
  dz = 1;
  dyaw = 1;

  Vx = 1;
  Vy = 1;
  Vz = 1;
  yawRate = 1;

    ACK::ErrorCode subscribeStatus;
    subscribeStatus=vehicle->subscribe->verify(responseTimeout);
    if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
      ACK::getErrorCodeMessage(subscribeStatus, __func__);
      return false;
    }
    int pkgIndex                  = 0;//就是一个占位符
    int freq                      = 50;
    TopicName topicList50Hz[] = { TOPIC_RC, TOPIC_RC_FULL_RAW_DATA };
    int numTopic                  = sizeof(topicList50Hz) / sizeof(topicList50Hz[0]);
    bool enableTimestamp           = false;

    bool pkgStatus = vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicList50Hz, enableTimestamp, freq);
    if (!(pkgStatus)){
      return pkgStatus;
    }
    subscribeStatus = vehicle->subscribe->startPackage(pkgIndex, responseTimeout);
    if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
    {std::cout<<"got the authority"<<std::endl;
      ACK::getErrorCodeMessage(subscribeStatus, __func__);
      // Cleanup before return
      vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
      return false;
    }
    TypeMap<TOPIC_RC>::type rc;
    TypeMap<TOPIC_RC_FULL_RAW_DATA>::type RCFullRawData;
    std::cout<<"subscribe init successfully!"<<std::endl;
    while(1){//不断扫描模式

      rc = vehicle->subscribe->getValue<TOPIC_RC>();//enter the circle again ,only to get the subscription once ,then core dump
      //there need to subscrip again
      RCFullRawData = vehicle->subscribe->getValue<TOPIC_RC_FULL_RAW_DATA>();
      //usleep(5000);
      usleep(20000);
      rcMode = rc.mode;
      // sbusData = RCFullRawData.sbus.data;  
      cout<<"the mode is"<<rcMode<<std::endl<<std::endl;
      cout<<"the SbusData is"<<std::endl;
      // for (int i =0 ;i<16;i++)cout<<"第"<<i<<"通道 :"<<sbusData[i]<<std::endl;
      for (int i =0 ;i<16;i++)cout<<"第"<<i<<"通道 :"<<RCFullRawData.sbus.data[i]<<std::endl;
      switchChannl = RCFullRawData.sbus.data[7];//可以改引用，但是我忘了
      if(switchChannl > 200 && switchChannl < 1000 && rc.mode < 7000){
        cout<<"not in the mode P,can not get ctrl"<<endl;
      }
      if(switchChannl > 200 && switchChannl < 1000 && rcMode > 7000){
        if(AuthorityFlag == false){
          vehicle->obtainCtrlAuthority(functionTimeout);//夺取控制权
          cout<<"get the CtrlAuthority"<<endl;
          AuthorityFlag = true;
        }

          // moveByPositionOffset(vehicle, dx, dy, 0, dyaw);//控制飞一定路径，完成控制会返回true
          // cout<<"using positionAndYawCtrl"<<endl;
          // vehicle->control->positionAndYawCtrl(dx, dy,dz, dyaw);
        cout<<"using velocityAndYawRateCtrl"<<endl;
        vehicle->control->velocityAndYawRateCtrl(Vx, Vy, 0, 0);
        //rc = vehicle->subscribe->getValue<TOPIC_RC>();
        //RCFullRawData = vehicle->subscribe->getValue<TOPIC_RC_FULL_RAW_DATA>();
        //usleep(5000);
        //rcMode = rc.mode;
        //switchChannl = RCFullRawData.sbus.data[7];
      }
        if(rcMode < 7000 ||switchChannl < 200 || switchChannl>1000){
		if(AuthorityFlag){
          vehicle->releaseCtrlAuthority();
          AuthorityFlag = false;
          cout<<"release the CtrlAuthority"<<endl;
	  usleep(100000);
		}
        }
    //}
    }
  vehicle->subscribe->removePackage(0, responseTimeout);
  cout<<"quit the circle"<<endl;
  return 0;
}
